package dipl.algorithm.math.fp.curve;

import dipl.algorithm.math.primitive.Side;
import dipl.algorithm.math.primitive.Tuple2;
import dipl.algorithm.exception.ArgumentException;
import dipl.algorithm.exception.ArgumentNullException;
import dipl.algorithm.math.fp.geometry.op.Point2dSetOps;
import dipl.algorithm.math.fp.geometry.BoundingWedge2d;
import dipl.algorithm.math.fp.primitive.Interval1df;
import dipl.algorithm.math.fp.primitive.Point2df;
import dipl.algorithm.math.fp.primitive.Vector2df;
import dipl.algorithm.math.fp.primitive.op.ComplexOps;
import dipl.algorithm.math.fp.primitive.op.Vector2dOps;
import java.util.LinkedList;

/**
 * Represents a bezier curve of arbitrary degree in the plane
 */
public class BezierCurve {

	//
	// CONSTRUCTOR METHODS
	//

	/**
	 * Constructs curve of degree n
	 * @param degree
	 * @throws ArgumentException degree is smaller than zero
	 */
	public BezierCurve( int degree ) throws ArgumentException {
		if( degree < 0 )
			throw new ArgumentException( "Degree must be greater or equal zero!" );

		this.degree = degree;
		this.coeff = null;
		this.domain = Interval1df.UnitInterval();

		this.controls = new Point2df[degree+1];
		for( int i = 0; i <= degree; i++ )
			this.controls[i] = new Point2df();
	}

	/**
	 * Constructs an exact copy of given curve c
	 * @param c
	 * @throws ArgumentNullException c is null
	 */
	public BezierCurve( BezierCurve c ) throws ArgumentNullException {
		if( c == null )
			throw new ArgumentNullException();

		this.degree = c.degree;
		this.domain = new Interval1df( c.domain );
		this.coeff = null;
		if( c.coeff != null ) {
			this.coeff = new double[c.coeff.length];
			for( int i = 0; i < this.coeff.length; i++ )
				this.coeff[i] = c.coeff[i];
		}
		this.controls = new Point2df[degree+1];
		for( int i = 0; i <= degree; i++ )
			controls[i] = new Point2df( c.controls[i] );
	}

	/**
	 * Constructs curve from given control points
	 * @param controls
	 * @throws ArgumentNullException control point array is null
	 * @throws ArgumentException control points array is empty
	 */
	public BezierCurve( Point2df[] controls ) throws ArgumentNullException, ArgumentException {
		if( controls == null )
			throw new ArgumentNullException( "Control points array is null!" );
		if( controls.length < 1 )
			throw new ArgumentException( "Control points array is empty!" );

		degree = controls.length-1;
		coeff = null;
		domain = Interval1df.UnitInterval();

		this.controls = controls;
	}

	//
	// PROPERTIES
	//

	/**
	 * @return control points of curve
	 */
	public Point2df[] Controls() {
		return controls; 
	}

	/**
	 * Sets control polygon of curve
	 * @param controls
	 * @throws ArgumentException
	 * @throws ArgumentNullException
	 */
	public void SetControls( Point2df[] controls ) throws ArgumentException, ArgumentNullException {
		if( controls == null )
			throw new ArgumentNullException();
		if( controls.length != degree+1 )
			throw new ArgumentException( "Number of control points must equal degree+1!" );
		this.controls = controls;
	}

	/**
	 * @return degree of curve's polynom
	 */
	public int Degree() {
		return degree;
	}

	/**
	 * @return parameter domain
	 */
	public Interval1df Domain() {
		return domain;
	}

	/**
	 * Sets parameter domain
	 * @param domain
	 */
	public void SetDomain( Interval1df domain ) {
		this.domain = domain;
	}

	//
	// PUBLIC METHODS
	//

	/**
	 * Calculates point on bezier-curve at parameter t
	 * @param fpMath
	 * @param t
	 * @return
	 */
	public Point2df Calculate( double t ) {
		return CalculatePointDeCasteljau( t );
	}

	/**
	 * Calculates n points at parameter values {a,a+e,a+2e,...,b} where e=(b-a)/n.
	 * @param fpMath fixed precision arithmetic
	 * @param points
	 */
	public void CalculatePoints( Point2df[] points ) throws ArgumentException {
		if( points == null || points.length == 0 )
			return;

		int n = points.length-1; // index of last point to calculate
		double t = 0.0; // current local parameter
		double deltaT = 1.0/(double)n; // parameter increment

		Point2df point = new Point2df(); // current point calculated

		for( int k = 0; k <= n; k++ ) {
			CalculatePointDeCasteljau( t, point );
			points[k].x = point.x;
			points[k].y = point.y;
			t += deltaT;
		}
	}

	/**
	 * @return whether or not curve can be extrapolated (only for testing purposes, since bezier-curves
	 * can be extrapolated)
	 */
	public boolean CanBeExtrapolated() {
		return false;
	}

	/**
	 * Returns a Beziér curve representing the part of this curve in given domain [c,d].
	 * Let domain of this curve be [a,b]
	 * WARNING: d and a must not coincide! If that may happen use {@link Extrapolate()} instead.
	 * @see {@link ChangeDomain()} for more information
	 * @param fpMath
	 * @param newDomain
	 * @return
	 */
	public BezierCurve ChangeDomain( Interval1df newDomain ) {
		// create resulting curve with same degree
		BezierCurve res = null;
		try {
			res = new BezierCurve( degree );
		}
		catch( Exception e ) {
			e.printStackTrace();
		}
		res.SetDomain( newDomain );

		// this array will take all intermediate points calculated
		Point2df[] points = new Point2df[controls.length];
		for( int i = 0; i < controls.length; i++ ) {
			points[i] = new Point2df( controls[i] );
		}

		// first of all generate curve for [a,d]
		double td = (newDomain.U()-domain.L())/domain.Length();
		DeCasteljauUpperDiag( td, points, res.controls );

		// finally get part of [a,d]-curve representing [c,d]-part
		double tc = (newDomain.L()-domain.L())/(newDomain.U()-domain.L());
		// restore starting points for Casteljau
		int len = res.controls.length;
		for( int i = 0; i < len; i++ ) {
			points[i].Set( res.controls[i] );
		}
		DeCasteljauLowerDiag( tc, points, res.controls );
		return res;
	}

	/**
	 * Returns whether given curve c equals this curve.
	 * Curves are equal if they have the same control polygon
	 * @param c
	 * @return
	 */
	public boolean Equals( BezierCurve c ) {
		try {
			return Point2dSetOps.AreEqual( controls, c.controls );
		}
		catch( Exception e ) {
			return false;
		}
	}

	/**
	 * Extrapolates curve using De Casteljaus's algorithm.
	 * let domain of curve by [a,b]
	 * If c >= 1 curve is extrapolated to [b,a+(b-a)*c]
	 * If c <= 0 curve is extrapolated to [a+(b-a)*c,a]
	 * @param fpMath
	 * @param c
	 * @return
	 */
	public BezierCurve Extrapolate( double c ) throws ArgumentException {
		if( c > 0.0 || c < 1.0 )
			throw new ArgumentException( "c MUST not be in (0.0,1.0)!" );
		BezierCurve resCurve = null;
		try {
			resCurve = new BezierCurve( degree );
		} catch( Exception e ) {
			e.printStackTrace();
		}

		if( c > 1.0 )
			resCurve.domain = new Interval1df( domain.U(), domain.L()+c*domain.Length() );

		else
			resCurve.domain = new Interval1df( domain.L()+c*domain.Length(), domain.L() );

		// temporary array for calculation of new control points
		Point2df[] points = new Point2df[controls.length];
		for( int i = 0; i < controls.length; i++ ) {
			points[i] = new Point2df( controls[i] );
		}

		if( c >= 0.0 )
			DeCasteljauLowerDiag( c, points, resCurve.controls );
		else
			DeCasteljauUpperDiag( c, points, resCurve.controls );
		return resCurve;
	}

	/**
	 * Calculates hodograph (Derivative) or bezier-curve.
	 * If curve has degree zero, <code>null</code> is returned.
	 * @param fpMath
	 * @param scaleDown scaledown by degree?
	 * @return
	 */
	public BezierCurve Hodograph( boolean scaleDown ) {
		if( degree == 0 )
			return null; // TODO return constant zero function here

		BezierCurve hodograph = null;
		try { hodograph = new BezierCurve( degree-1 ); } catch( Exception e ) {}
		hodograph.domain = this.domain;
		// simply calculate new control points
		double fdeg = scaleDown ? 1.0 : (double)degree;
		for( int i = 0; i < degree; i++ ) {
			hodograph.controls[i].x = fdeg*(controls[i+1].x-controls[i].x);
			hodograph.controls[i].y = fdeg*(controls[i+1].y-controls[i].y);
		}
		return hodograph;
	}

	/**
	 * Returns whether curve is degenerated to one point
	 * @return
	 */
	public boolean IsDegenerated() {
		return (degree == 0 || Point2dSetOps.IsOnePoint( controls ));
	}

	public Tuple2<BezierCurve,BezierCurve> Subdivide( double t0 ) throws ArgumentException {
		if( t0 < 0.0 || t0 > 1.0 )
			throw new ArgumentException( "Parameter \"t0\" must be in between 0.0 and 1.0!" );

		BezierCurve c0 = new BezierCurve( degree );
		c0.domain = new Interval1df( domain.L(), domain.L()+t0*domain.Length() );
		BezierCurve c1 = new BezierCurve( degree );
		double oneMinusT0 = 1.0-t0;
		c1.domain = new Interval1df( domain.U()-oneMinusT0*domain.Length(), domain.U() );

		// this array will take all intermediate points calculated
		Point2df[] points = new Point2df[controls.length];
		int i; // indices
		for( i = 0; i < controls.length; i++ ) {
			points[i] = new Point2df( controls[i] );
		}

		// first control point of first new curve is simply taken from old curve
		c0.controls[0].Set( points[0] );

		// last control point of second new curve is simply taken from old curve
		c1.controls[degree].Set( points[degree] );

		if( degree == 0 )
			return new Tuple2<BezierCurve, BezierCurve>( c0, c1 );

		int k; // indices

		// Calc DeCastlejau approximations
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				points[i].x = oneMinusT0*points[i].x+t0*points[i+1].x;
				points[i].y = oneMinusT0*points[i].y+t0*points[i+1].y;
			}

			// curve0 controls are points on diagonal P(i,0)
			// curve1 controls are points on diagonal P(i,deg-i)
			c0.controls[k].Set( points[0] );
			c1.controls[degree-k].Set( points[degree-k] );
		}
		return new Tuple2<BezierCurve, BezierCurve>( c0, c1 );
	}

	/**
	 * Sets ith control point to given coordinates (x,y)
	 * @param i index of control point (0..degree)
	 * @param x
	 * @param y
	 */
	public void SetControlPoint( int i, double x, double y ) {
		controls[i].x = x;
		controls[i].y = y;
	}

	/**
	 * Tries to calculate boundign wedge containing tangent vectors for given control points of a curve.
	 * It is assumed that controls contain at least two points and at least two points differ (original curve is not degnerated).
	 * The method used here not always finds the exact wedge!
	 * @param fpMath fixed-precision math
	 * @param controls control points
	 * @returns wedge or <code>null</code> if number of controls is lesser or equal one!
	 */
	public static BoundingWedge2d BoundingWedge( Point2df[] controls ) {
		// no wedge for degree 0 because hodograph is zero!
		int deg = controls.length-1;
		if( deg <= 0 )
			return null;
		int i;
		// construct hodograph
		Point2df[] hodoVtcs = new Point2df[deg];
		Point2df temp;
		for( i = 0; i < deg; i++ ) {
			temp = new Point2df( controls[i+1].x-controls[i].x , controls[i+1].y-controls[i].y );
			hodoVtcs[i] = temp;
		}
		// construct wedge of vectors above and below x-axis
		Vector2df al = null, ar = null; // directions of left and right (clockwise) bounding vectors of above wedge
		Vector2df bl = null, br = null; // directions of left and right (clockwise) bounding vectors of below wedge

		// above
		// find vector above and below x-axis to start with
		LinkedList<Vector2df> above = new LinkedList<Vector2df>();
		LinkedList<Vector2df> below = new LinkedList<Vector2df>();
		for( i = 0; i < deg; i++ ) {
			if( hodoVtcs[i].y >= 0.0 ) {
				above.addLast( new Vector2df( hodoVtcs[i].x, hodoVtcs[i].y ) );
			}
			else {
				below.addLast( new Vector2df( hodoVtcs[i].x, hodoVtcs[i].y ) );
			}
		}

		Side s; // temporary

		if( above.size() > 0 ) {
			al = ar = above.removeFirst();
			for( Vector2df v : above ) {
				s = Vector2dOps.WhichSide( al, v );
				if( s == Side.Left ) {
					al = v;
					continue;
				}
				s = Vector2dOps.WhichSide( ar, v );
				if( s == Side.Right ) {
					ar = v;
				}
			}
		}

		// no vectors below x-axis -> we are done
		if( below.size() == 0 ) {
			return new BoundingWedge2d( new Vector2df( al ), Vector2dOps.Angle( al, ar ) );
		}

		bl = br = below.removeFirst();
		for( Vector2df v : below ) {
			s = Vector2dOps.WhichSide( bl, v );
			if( s == Side.Left ) {
				bl = v;
				continue;
			}
			s = Vector2dOps.WhichSide( br, v );
			if( s == Side.Right ) {
				br = v;
			}
		}

		// no vectors found above x-axis -> we are done
		if( al == null ) {
			return new BoundingWedge2d( new Vector2df( bl ), Vector2dOps.Angle( bl, br ) );
		}

		// both above and below contain vectors
		double angle_al = Math.PI-ComplexOps.Argument( al.x, al.y ); // (cw-) angle of al with negative x-axis
		double angle_ar = ComplexOps.Argument( ar.x, ar.y ); // (ccw-) angle of ar with positive x-axis
		double angle_bl = 2.0*Math.PI-ComplexOps.Argument( bl.x, bl.y ); // (cw-) angle of bl with positive x-axis
		double angle_br = ComplexOps.Argument( br.x, br.y )-Math.PI; // (ccw-) angle of br with negative x-axis
		// full wedge?
		if( angle_al+angle_br <= Math.PI &&
				angle_ar+angle_bl  <= Math.PI ) {
			return new BoundingWedge2d( new Vector2df( 1.0, 0.0 ), 2.0*Math.PI );
		}
		// wedge opening to the negative x-axis
		else if( (angle_al+angle_br) <= Math.PI ) {
			return new BoundingWedge2d( new Vector2df( bl ), 2.0*Math.PI-(angle_ar+angle_bl) );

		}
		// wedge opening to the positive x-axis
		else {
			return new BoundingWedge2d( new Vector2df( al ), 2.0*Math.PI-(angle_al+angle_br) );
		}
	}

	/**
	 * Tries to calculate bounding wedge containing tangent vectors for this curve.
	 * Assumes that given curve is not degenerated.
	 * The method used here not always finds the exact wedge!
	 * @param fpMath fixed-precision math
	 * @param length length of wedge bounding vector
	 * @returns wedge or <code>null</code> if degree of curve is 0
	 */
	public BoundingWedge2d BoundingWedge() {
		// no tangent wedge for degree 0 because hodograph is zero!
		if( degree == 0 )
			return null;
		return BoundingWedge( controls );
	}

	@Override
	public String toString() {
		return "["+"Degree: "+this.degree+", Domain: "+this.domain+"]";
	}
	//
	// AUXILLIARY METHODS
	//

	/**
	 * Calculates Bernstein polynom coefficients and puts result in coefficients-array
	 */
	private void CalculateCoefficients() {
		coeff[0] = 1.0;
		for( int i = 1; i <= degree; i++ ) {
			coeff[i] = (coeff[i-1]*(double)(degree-i+1))/(double)i;
		}
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) directly by
	 * evaluating the bernstein polynomials. This method is numerically not very stable.
	 * @param fpMath
	 * @param t
	 * @param point resulting point
	 */
	private void CalculatePointBernstein( double t, /*out*/ Point2df point ) {
		point.x = 0.0;
		point.y = 0.0;
		int i, j; // indices
		double polyval; // value of bernstein-polynomial

		// one time calculation of bernstein-polynomial-coefficents
		// if not already done
		if( coeff == null ) {
			coeff = new double[degree+1];
			CalculateCoefficients();
		}

		for( i = 0; i <= degree; i++ ) {
			// calculate blending value for i
			polyval = coeff[i];
			for( j = 1; j <= i; j++ )
				polyval = polyval*t;
			for( j = 1; j <= degree-i; j++ )
				polyval = polyval*(1.0-t);
			point.x = point.x+(controls[i].x*polyval);
			point.y = point.y+(controls[i].y*polyval);
		}
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) using
	 * De Casteljau's algorithm.
	 * This method is numerically more stable than calculating the Bernstein polynomial directly
	 * but its not as memory efficient.
	 * @param fpMath
	 * @param c
	 * @return
	 */
	private Point2df CalculatePointDeCasteljau( double c ) {
		Point2df ret = new Point2df();
		CalculatePointDeCasteljau( c, ret );
		return ret;
	}

	/**
	 * Calculates the point on the bezier-curve at parameter "t" (in [0.0 .. 1.0]) using
	 * De Casteljau's algorithm.
	 * This method is numerically more stable than calculating the Bernstein polynomial directly 
	 * but its not as memory efficient.
	 * @param fpMath
	 * @param c
	 * @param point
	 */
	private void CalculatePointDeCasteljau( double c, /*out*/ Point2df point ) {
		int i, k; // indices
		double oneMinusC = 1.0-c;
		// this array will take all intermediate points calculated.
		// this is what makes this method less memory-efficient
		Point2df[] points = new Point2df[controls.length];
		for( i = 0; i < controls.length; i++ ) {
			points[i] = new Point2df( controls[i] );
		}

		// Calc DeCastlejau approximations
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				points[i].x =  oneMinusC*points[i].x+c*points[i+1].x;
				points[i].y = oneMinusC*points[i].y+c*points[i+1].y;
			}
		}

		point.x = points[0].x;
		point.y = points[0].y;
	}

	/**
	 * Helper method to calculate the intermediate points b(i,n-i) in the "lower" line
	 * of the De-Casteljau-triangle at parameter c.
	 * The resulting points are ordered: [b(n,0), b(n-1,1),...,#b(0,n)]
	 * Triangle expample:
	 * b(0,0)
	 *         b(1,0)
	 * b(0,1)         #b(2,0)
	 *         #b(1,1)
	 * #b(0,2)
	 * @param fpMath
	 * @param c
	 * @param intermPoints intermediate triangle points
	 * @param resDiagPoints resulting lower diagonal
	 */

	private void DeCasteljauLowerDiag( double c, Point2df[] intermPoints, /*out*/ Point2df[] resDiagPoints ) {

		resDiagPoints[degree].Set( intermPoints[degree] );

		if( degree == 0 )
			return;

		int i, k; // indices
		double oneMinusC = 1.0-c;

		// Calc DeCastlejau interm points
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				intermPoints[i].Set(
					oneMinusC*intermPoints[i].x+c*intermPoints[i+1].x,
					oneMinusC*intermPoints[i].y+c*intermPoints[i+1].y
				);
			}

			// controls are points on diagonal b(i,0)
			resDiagPoints[degree-k].Set( intermPoints[degree-k] );
		}
	}

	/**
	 * Helper method to calculate the intermediate points b(i,0) in the "upper" line
	 * of the De-Casteljau-triangle at local parameter c.
	 * The resulting points are ordered: [b(0,0), b(1,0),...,#b(n,0)]
	 * Triangle expample:
	 * #b(0,0)
	 *        #b(1,0)
	 * b(0,1)        #b(2,0)
	 *        b(1,1)
	 * b(0,2)
	 * @param fpMath
	 * @param c
	 * @param intermPoints intermediate triangle points
	 * @param resDiagPoints resulting upper diagonal
	 */
	private void DeCasteljauUpperDiag( double c, Point2df[] intermPoints, /*out*/ Point2df[] resDiagPoints ) {

		resDiagPoints[0].Set( intermPoints[0] );

		if( degree == 0 )
			return;

		int i, k; // indices
		double oneMinusC = 1.0-c;

		// Calc DeCastlejau interm points
		for( k = 1; k <= degree; k++ ) {
			for( i = 0; i <= degree-k; i++ ) {
				intermPoints[i].Set(
					oneMinusC*intermPoints[i].x+c*intermPoints[i+1].x,
					oneMinusC*intermPoints[i].y+c*intermPoints[i+1].y
				);
			}

			// controls are points on diagonal b(i,0)
			resDiagPoints[k].Set( intermPoints[0] );
		}
	}

	//
	// MEMBERS
	//

	/**
	 * degree of curve
	 */
	private int degree;
	/**
	 * coefficients in bernstein polynomial (C(n,k))
	 */
	private double[] coeff;
	/**
	 * control points
	 */
	private Point2df[] controls;
	/**
	 * parameter domain
	 */
	Interval1df domain;
}
